package source.realRobot;

import java.util.ArrayList;
import java.util.List;

import source.calcCmd.State;
import source.calcCmd.Vec;
import source.calcCmd.RealLifeProperties;
import source.calcCmd.RobotCommand;
import source.Enums.Direction;
import source.realRobot.uC.TimerCnt;
import source.realRobot.uC.UC;
import source.Enums.Side;

public class RealRobot{

	private UC uC;
	private Motor motorL;
	private Motor motorR;
	private Wheel wheelL;
	private Wheel wheelR;
	
	private int prevSpeedL;
	private int prevSpeedR;
	
	private Vec pos;
	private double orient;
	
	private List<Vec> path;
	private List<Vec> idealPath;
	private double lastIdealOrient;
	private List<Vec> cmds;
	private List<Vec> idealCmds;
	
	private State lastIdealState;
	
	public boolean idealInc = true;
	
	private State state;
	
	public RealRobot() {
		
		wheelL = new Wheel();
		wheelR = new Wheel();
		
		motorL = new Motor(wheelL);
		motorR = new Motor(wheelR);
		
		uC = new UC(this,motorL,motorR);
		
		pos = new Vec(0,0);
		path = new ArrayList<Vec>(1000);
		idealPath = new ArrayList<Vec>(500);
		cmds = new ArrayList<Vec>(5);
		idealCmds = new ArrayList<Vec>(5);
		orient = 0;
		lastIdealOrient = 0;
		prevSpeedL = TimerCnt.maxValue;
		prevSpeedR = TimerCnt.maxValue;
		
		lastIdealState = new State(new Vec(0,0),0,0,0);
		state = new State(new Vec(0,0),0,0,0);
	}
	
	public State runCmd(State startState, RobotCommand cmd){
		pos = startState.pos;
		
		wheelL.setV(Math.abs(RealLifeProperties.speedToV(cmd.getPrevSpeed(Side.LEFT))));
		wheelL.setDir(cmd.getDirL());
		wheelR.setV(Math.abs(RealLifeProperties.speedToV(cmd.getPrevSpeed(Side.RIGHT))));
		wheelR.setDir(cmd.getDirR());
		orient = startState.fi;

		
		uC.setTimers(	RealLifeProperties.vToSpeed(startState.vL),
						RealLifeProperties.vToSpeed(startState.vR) );

		uC.runCmd(cmd);
		
		prevSpeedR = uC.getSpeed(Side.RIGHT);
		prevSpeedL = uC.getSpeed(Side.LEFT);
		
		return new State(new Vec(pos),orient,wheelL.getV(),wheelR.getV());
	}
	
	public void setState(State state){
		orient = state.fi;
		pos = new Vec(state.pos);
		wheelL.setV(Math.abs(state.vL));
		/*-
		if (state.vL < 0){
			wheelL.setDir(Direction.BACKWARD);
		} else {
			wheelL.setDir(Direction.FORWARD);
		}
		wheelR.setV(Math.abs(state.vR));
		if (state.vR < 0){
			wheelR.setDir(Direction.BACKWARD);
		} else {
			wheelR.setDir(Direction.FORWARD);
		}
		*/
	}
	
	public static State idealMove(State start, long nanoTime){
		// radius in meters
		double r;
		// angular velocity
		double omega;
		// A vector pointing to the center of rotation
		Vec toO;
		// Rotation center
		Vec o;
		// Rotation in radians
		double alpha;
		// t in SEC
		double ts = nanoTime / RealLifeProperties.s2ns;
		// the function makes modofications on this "virtual" state
		State ret = start;
		
		// If the trajectory is not linear
		if (Math.abs(ret.vR-ret.vL)>0.001){
			r = RealLifeProperties.l*(ret.vL+ret.vR)/(ret.vR-ret.vL);
			
			omega = (ret.vR-ret.vL)/(2*RealLifeProperties.l);
			// toO = [-sin(fi);cos(fi)]
			toO = new Vec(-1*(Math.sin(ret.fi)),Math.cos(ret.fi));
			// o = ret +  r*toO
			o = new Vec(ret.pos.addNR(toO.multiply(r)));
			
			alpha = omega * ts;
			
			Vec tmp = ret.pos.add(o.multiplyNR(-1));
			// new position
			ret.pos = o.add(new Vec(tmp.getX()*Math.cos(alpha)-tmp.getY()*Math.sin(alpha),
							tmp.getX()*Math.sin(alpha)+tmp.getY()*Math.cos(alpha)));
			// new orientation
			ret.fi += alpha;
		} else {
			double v = ret.vR;
			Vec i = new Vec((Math.cos(ret.fi)),Math.sin(ret.fi));
			ret.pos.add(i.multiplyNR(v*ts));
		}
		
		ret.fi = RealLifeProperties.normalizeOrient(ret.fi);
		
		return ret;
	}
	
	public void move(long time){
		// Vec vec, double orient, double vR, double vL
		state.setState(pos,orient,wheelR.getV(),wheelL.getV());
		
		setState(idealMove(state,time));
	}
	
	public State getLastIdealState(){
		return lastIdealState;
	}
	
	private void calcIdealPath(RobotCommand cmd){
		State start = new State(pos,orient,wheelR.getV(),wheelL.getV());
		
		
		if (cmd.getStepsL() == 0 && cmd.getStepsR() == 0){
			return;
		}
		
		wheelR.setV(Math.abs(cmd.getRealV(Side.RIGHT)));
		wheelR.setDir(cmd.getDirR());
		wheelL.setV(Math.abs(cmd.getRealV(Side.LEFT)));
		wheelL.setDir(cmd.getDirL());
		
		if (cmd.getStepsL() == 0){
			wheelL.setV(0);
		}
		if (cmd.getStepsR() == 0){
			wheelR.setV(0);
		}
		
		orient = lastIdealOrient;
		
		if (idealPath.size() > 0){
			pos = idealPath.get(idealPath.size()-1);
			idealCmds.add(pos);
		}
		
		idealPath.add(pos);
		int gran = 100;
		long time = (long) (cmd.getRealTime()*RealLifeProperties.s2ns/1000.0);
		
		long tick = time/gran;
		for (int i = 0; i < gran; i++){
			move(tick);
			idealPath.add(pos);
		}
		
		idealCmds.add(pos);
		lastIdealOrient = orient;
		lastIdealState = new State(new Vec(pos.x,pos.y),orient,0,0);
		setState(start);
	}
	
	public void addToCmdList(){
		cmds.add(pos);
	}
	
	public List<Vec> getCmds(){
		return cmds;
	}
	
	public List<Vec> getIdealCmds(){
		return idealCmds;
	}
	
	public double rotate(double fi, double angle){
		double ret = fi;
		ret+=angle;
		return ret;
	}
	
	public int getPrevSpeed(Side which){
		int value = 0;
		
		switch (which){
		case LEFT:
			value = prevSpeedL;
			break;
		case RIGHT:
			value = prevSpeedR;
			break;
		}
		
		return value;
	}
	
	public void stopWheel(Side which){
		switch (which){
		case LEFT:
			wheelL.setV(0);
			break;
		case RIGHT:
			wheelR.setV(0);
			break;
		}
	}
	
	public Direction getDir(Side which){
		double value = 0;
		
		switch (which){
		case LEFT:
			value = wheelL.getDir();
			break;
		case RIGHT:
			value = wheelR.getDir();
			break;
		}
		
		if (value == 1){
			return Direction.FORWARD;
		} else {
			return Direction.BACKWARD;
		}
	}
	
	public double getV(Side which){
		double value = 0;
		
		switch (which){
		case LEFT:
			value = wheelL.getV();
			break;
		case RIGHT:
			value = wheelR.getV();
			break;
		}
		
		return value;
	}
	
	public void addToPath(){
		path.add(pos);
	}
	
	public List<Vec> getPath(){
		return path;
	}
	
	public List<Vec> getIdealPath(){
		return idealPath;
	}
	
	public void reset(){
		pos = new Vec(0,0);
		orient = 0;
		motorL.reset();
		motorR.reset();
		wheelL.reset();
		wheelR.reset();
		uC.reset();
		path = new ArrayList<Vec>(5000);
		idealPath = new ArrayList<Vec>(1000);
		lastIdealOrient = 0;
		cmds = new ArrayList<Vec>(5);
		idealCmds = new ArrayList<Vec>(5);
	}
	
	public void simpleReset(){
		pos.x = 0;
		pos.y = 0;
		orient = 0;
		motorL.reset();
		motorR.reset();
		wheelL.reset();
		wheelR.reset();
		uC.reset();
	}
	
	public double getX(){
		return pos.x;
	}
	
	public double getY(){
		return pos.y;
	}
	
	public double getOrient(){
		return orient;
	}

}
